#ifndef LIDAR_FUSION_CORE_H
#define LIDAR_FUSION_CORE_H

#define _APP_NAME_ "lidar_fusion"

#include "ros/ros.h"
#include "tf/tf.h"
#include "eigen3/Eigen/Core"
#include <eigen3/Eigen/LU>
#include "sensor_msgs/PointCloud2.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/PoseStamped.h"
#include "pcl_ros/transforms.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

class LidarFusion{
    public:
        LidarFusion();
        ~LidarFusion();
        void run();
    
    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;
    
    private:
        tf2_ros::Buffer buffer_;
        tf2_ros::TransformListener listener_;
    
    private:
        std::string input_front_lidar_topic_;
        std::string input_front_lidar_topic_frame_id;
        std::string input_back_lidar_topic_;
        std::string input_back_lidar_topic_frame_id;
        std::string output_confusion_lidar_topic_;
        std::string output_confusion_lidar_frame_id;
    
    
    private:
        sensor_msgs::PointCloud2 front_lidar_pc_trans;
        sensor_msgs::PointCloud2 back_lidar_pc_trans;

    private:
        ros::Subscriber front_lidar_sub_;
        ros::Subscriber back_lidar_sub_;
        ros::Publisher confusion_lidar_pub_;
    
    private:
        void front_lidar_call_back(const sensor_msgs::PointCloud2::ConstPtr& msg);
        void back_lidar_call_back(const sensor_msgs::PointCloud2::ConstPtr& msg);
        void process(const ros::TimerEvent& e);
};

#endif